#include "location.hpp"

void rc_in_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
    channel9_value = rcvalue.channels[8];
    channel10_value = rcvalue.channels[9];
}

void gps_callback(const sensor_msgs::NavSatFix &msg)
{
    shouchiduan_gps_pos.latitude = msg.latitude;
    shouchiduan_gps_pos.longitude = msg.longitude;
    //replace asml to rel height
    shouchiduan_gps_pos.altitude = msg.altitude;
    // shouchiduan_gps_pos.altitude = current_point_z;
    gps_yuandian_lon = shouchiduan_gps_pos.longitude-current_point.x/(111318.0*cos(shouchiduan_gps_pos.latitude*(3.14159265/180.0)));
    gps_yuandian_lat = shouchiduan_gps_pos.latitude-current_point.y/110946.0;
    gps_yuandian_alt = shouchiduan_gps_pos.altitude-current_point.z;


}
void vehicleodomCallback(const sensor_msgs::NavSatFix &msg) 
{
    vechile_point.x= (msg.longitude - gps_yuandian_lon)*111318.0*cos((msg.latitude + gps_yuandian_lat)/2*deg2rad);;
    vechile_point.y= (msg.latitude - gps_yuandian_lat)*110946.0;
    vechile_point.z= msg.altitude;

    track_point.x= (msg.position_covariance[2] - gps_yuandian_lon)*111318.0*cos((msg.position_covariance[1] + gps_yuandian_lat)/2*deg2rad);
    track_point.y= (msg.position_covariance[1] - gps_yuandian_lat)*110946.0;
    track_point.z= msg.position_covariance[3];



    Box box;
    box.x_min = vechile_point.x-0.5;
    box.y_min = vechile_point.y-0.5;
    box.z_min = vechile_point.z-0.5;
    box.x_max = vechile_point.x+0.5;
    box.y_max = vechile_point.y+0.5;
    box.z_max = vechile_point.z+0.5;
    // std::cout<<"--->test1: "<<msg.position_covariance[4]<<std::endl;
    if(msg.position_covariance[4]==1)
    {
        draw_box(box, 1, 0.2, 0, 1, 0);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = vechile_point.x;
        draw_name_temppose.pose.position.y = vechile_point.y;
        draw_name_temppose.pose.position.z = vechile_point.z+1.0;
        draw_name(draw_name_temppose,"drone-track", 1);
    }
    else if(msg.position_covariance[4]==2)
    {
        draw_box(box, 1, 0.2, 0, 0, 1);
        geometry_msgs::PoseStamped draw_name_temppose;
        draw_name_temppose.pose.position.x = vechile_point.x;
        draw_name_temppose.pose.position.y = vechile_point.y;
        draw_name_temppose.pose.position.z = vechile_point.z+1.0;
        draw_name(draw_name_temppose,"drone-attack", 1);
        Box box2;
        box2.x_min = track_point.x-0.2;
        box2.y_min = track_point.y-0.2;
        box2.z_min = track_point.z-0.2;
        box2.x_max = track_point.x+0.2;
        box2.y_max = track_point.y+0.2;
        box2.z_max = track_point.z+0.2;
        draw_box(box2, 10, 0.2, 0, 1, 1);
    }
    
    

    
    
}

void localodomCallback(const nav_msgs::OdometryConstPtr &msg)
{
    current_point.x= msg->pose.pose.position.x;
    current_point.y= msg->pose.pose.position.y;
    // current_point.z= msg->pose.pose.position.z;
    current_point.z= 1.5;

    current_angle_Q = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,
                                    msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    current_angle.x= msg->pose.pose.orientation.x;
    current_angle.y= msg->pose.pose.orientation.y;
    current_angle.z= msg->pose.pose.orientation.z;
    current_angle.w= msg->pose.pose.orientation.w;
    // curr_angle = toEulerAngle(current_angle);
    // draw ray vector
    Eigen::Matrix3d Rpg_temp1 = current_angle_Q.toRotationMatrix();
    //draw vector
    geometry_msgs::Vector3 x_vector1_draw;
    x_vector1_draw.x = Rpg_temp1(0,0);
    x_vector1_draw.y = Rpg_temp1(1,0);
    x_vector1_draw.z = Rpg_temp1(2,0);
    visualization_msgs::Marker line_marker = draw_line_marker(x_vector1_draw, 100);
    marker2_pub.publish(line_marker);

    Eigen::Vector3d P_vehicle;
    
    P_vehicle(0) = current_point.x;
    P_vehicle(1) = current_point.y;
    P_vehicle(2) = current_point.z;

    Box box1;
    box1.x_min = P_vehicle(0)-0.5;
    box1.y_min = P_vehicle(1)-0.5;
    box1.z_min = P_vehicle(2)-0.5;
    box1.x_max = P_vehicle(0)+0.5;
    box1.y_max = P_vehicle(1)+0.5;
    box1.z_max = P_vehicle(2)+0.5;
    draw_box(box1, 3, 0.2, 1, 1, 0);
    geometry_msgs::PoseStamped draw_name_temppose1;
    draw_name_temppose1.pose.position.x = P_vehicle(0);
    draw_name_temppose1.pose.position.y = P_vehicle(1);
    draw_name_temppose1.pose.position.z = P_vehicle(2)+1.0;
    draw_name(draw_name_temppose1,"home", 3);


    // v_target_change = (channel10_value-1500)/100.0;
    double dt = ros::Time::now().toSec()-last_calc_time;
    if(dt>1.0) //too long maybe need init
    {
        last_calc_time = ros::Time::now().toSec();
        return;
    }
    dist_to_shouchi += v_target_change*dt;
    if(dist_to_shouchi<10)
    {
        dist_to_shouchi = 10;
    }

    // 定义局部前向向量（根据坐标系约定选择）
    Eigen::Vector3d local_forward(1.0, 0.0, 0.0); // x轴前向

    // 计算世界坐标系中的前向方向
    Eigen::Vector3d world_forward = Rpg_temp1 * local_forward;
    world_forward.normalize(); // 归一化

    // 计算位移并得到新坐标
    P_target = P_vehicle + world_forward * dist_to_shouchi;

    if(P_target(2)-P_vehicle(2)<3)
    {
        P_target(2) = P_vehicle(2)+3;
    }

    Box box2;
    box2.x_min = P_target(0)-0.5;
    box2.y_min = P_target(1)-0.5;
    box2.z_min = P_target(2)-0.5;
    box2.x_max = P_target(0)+0.5;
    box2.y_max = P_target(1)+0.5;
    box2.z_max = P_target(2)+0.5;
    draw_box(box2, 2, 0.2, 1, 0, 0);
    geometry_msgs::PoseStamped draw_name_temppose2;
    draw_name_temppose2.pose.position.x = P_target(0);
    draw_name_temppose2.pose.position.y = P_target(1);
    draw_name_temppose2.pose.position.z = P_target(2)+1.0;
    draw_name(draw_name_temppose2,"target", 2);

    
    odom_flag = true;
    last_calc_time = ros::Time::now().toSec();

    
}


void target_pub_callback(const ros::TimerEvent& /*event*/)
{
    if(!odom_flag)
    {
        return;
    }
    sensor_msgs::NavSatFix attack_target_gps_pos;
    attack_target_gps_pos.latitude = (P_target(1)/110946.0)+gps_yuandian_lat;
    attack_target_gps_pos.longitude = (P_target(0)/(111318.0*cos(gps_yuandian_lat*(3.14159265/180.0))))+gps_yuandian_lon;
    attack_target_gps_pos.altitude = P_target(2);
    target_pub.publish(attack_target_gps_pos);
}




int main(int argc, char **argv)
{
    ros::init(argc, argv, "ground_location");
    ros::NodeHandle nh;

    //read parameters from launch file
    nh.param<bool>("simulation",simulation,false);
    nh.param<double>("inflation",inflation,5.0);

    nh.param<double>("ground_height",ground_height,5.0);
    
    nh.param<double>("sample_size",sample_size,0.5);
    
    nh.param<double>("clusterTolerance",clusterTolerance,1.0);
    nh.param<int>("minsize",minsize,5);
    nh.param<int>("maxsize",maxsize,1000);

    nh.param<double>("min_lidar_distance",min_lidar_distance,2.0);
    nh.param<double>("max_lidar_distance",max_lidar_distance,100.0);
    nh.param<double>("lidar_intensity",lidar_intensity,50.0);
    nh.param<double>("track_search_field",track_search_field,1.0);
    



    local_position_sub = nh.subscribe("/mavros/local_position/odom",10,localodomCallback);
    gps_position_sub     = nh.subscribe("/mavros/global_position/global",10,gps_callback);
    rc_in_sub     = nh.subscribe("/mavros/rc/in",10,rc_in_callback);

    vechile_position_sub = nh.subscribe("/vehicle/position/global",10,vehicleodomCallback);


    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 10);
    marker2_pub = nh.advertise<visualization_msgs::Marker>("/ray_line", 10);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/drone_id_marker", 10);

    target_pub = nh.advertise<sensor_msgs::NavSatFix>("/zhou/target_gps", 10);

    target_pub_timer = nh.createTimer(ros::Duration(0.2), target_pub_callback);

    last_calc_time = ros::Time::now().toSec();
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(30);   


    int fd = open("/dev/input/event6", O_RDONLY | O_NONBLOCK);
    if (fd < 0) {
        perror("=======>Failed to open device");
        return 1;
    }
 
    // 读取事件循环（省略了设备初始化部分，因为它不是问题的关键）
    struct input_event ev;



    while(ros::ok()){

        ros::spinOnce();
        rate.sleep();


        ssize_t rc = read(fd, &ev, sizeof(struct input_event));
        if (rc == (ssize_t)sizeof(struct input_event)) {
            // std::cout<<"+++++>code: "<<ev.code<<std::endl;
            if(ev.code==30) //A
            {
                if(ev.value)
                {
                    v_target_change = 5.0;
                }
                else
                {
                    v_target_change = 0.0;
                }
                
            }
            else if(ev.code==31) //S
            {
                //预留键位
            }
            else if(ev.code==32) //D
            {
                if(ev.value)
                {
                    v_target_change = -5.0;
                }
                else
                {
                    v_target_change = 0.0;
                }
            }
        } else if (rc < 0 && errno != EAGAIN) {
            perror("Failed to read event...");
            break;
        }
    }

    close(fd);
 
    return 0;
}




visualization_msgs::Marker draw_line_marker(geometry_msgs::Vector3 vector, int length)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "vector_line";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = 1; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    // line_strip.lifetime = ros::Duration(0.1);
    line_strip.scale.x = 0.05; //width of the line
    line_strip.color.r = 0.1; line_strip.color.g = 0.2; line_strip.color.b = 0.5;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[2];
    p[0].x = current_point.x;     p[0].y = current_point.y;     p[0].z = current_point.z;
    p[1].x = current_point.x+(length*vector.x);     p[1].y = current_point.y+(length*vector.y);     p[1].z = current_point.z+(length*vector.z);


    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[1]);

    
    return line_strip;
    
    //end draw
}

void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(1);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs::Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw   
}
void draw_name(geometry_msgs::PoseStamped draw_pose,std::string name, int id)
{
    //draw target name 
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "chuzhi";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(1);
    marker_id.scale.z = 1.0;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 255;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  draw_pose.pose.position.x;
    name_pose.position.y =  draw_pose.pose.position.y;
    name_pose.position.z =  draw_pose.pose.position.z+1.0;
    marker_id.text= name;
    marker_id.pose= name_pose;
    marker_id_pub.publish(marker_id);
}
        